/**
 * @file        rte_bot_car.c
 * @brief       XXXX
 * @note        XXXX
 * @author      靳普诏
 * @date        2024/04/13
 * @version     1.0

 * @par         修改日志
 * <table>
 * <tr><th>Date         <th>Version     <th>Author      <th> Description
 * <tr><td>2024/04/13   <td>1.0         <td>靳普诏       <td> 创建初始版本
 * @copyright 
 */
 
#include "rte_bot_car.h"
#include "bsp_motor.h"
#include "bsp_motor_def.h"
#include "bsp_axis.h"
#include "bsp_laser.h"
#include "bsp_laser_def.h"
#include "osc_api.h"
#include "rte_bot_car_place.h"

#include <math.h>


#define RTE_BOT_CAR_MOTO_DIY_CTRL     1   // 电机自定义控制。 防止不想重新接线的问题

#define RTE_BOT_CAR_MOTIVE_POWER_MAX    (30)
#define RTE_BOT_CAR_MOTIVE_POWER_MIN    (20)

extern UInt32 Bsp_LaserOnRunOnce(void);



static Rte_TBotCar g_rte_bot_car_ = {0};

// ---------------------动力计算--------------------------

static Int32 L_Rte_TBotCarCalcSpinPwr(Rte_TBotCar *self, Int32 diff_angle)
{
    Int32 result = (Int32)Mdw_TPidCalc(&self->spin_pid_, diff_angle);
    return result;
}

static Int32 L_Rte_TBotCarCalcMotivePwr(Rte_TBotCar *self, Int32 distance)
{
    Bool flag = True;
    Int32 result = 0;
    
    if (distance < 0)
    {
        flag = False;
        distance = -distance;
    }
    
    result = distance == 0 ? 0 : RTE_BOT_CAR_MOTIVE_POWER_MIN;
    
    if (distance >= 500)
    {
        result = RTE_BOT_CAR_MOTIVE_POWER_MAX;
    }
    
    return flag ? result : -result;
}


#if (RTE_BOT_CAR_MOTO_DIY_CTRL)
static void L_Rte_TBotCarSetMotoPwr_FrontLeft(Rte_TBotCar *self, Int32 pwr)
{
    Bsp_THandle moto_handle = self->moto_rear_left_;
    if (pwr == 0)
    {
        Bsp_MotorStop(moto_handle);
    }
    else if (pwr > 0)
    {
        Bsp_MotorSetDirection(moto_handle, False);
        Bsp_MotorSetSpeed(moto_handle, pwr);
        Bsp_MotorStart(moto_handle);
    }
    else // if (pwr < 0)
    {
        Bsp_MotorSetDirection(moto_handle, True);
        Bsp_MotorSetSpeed(moto_handle, -pwr);
        Bsp_MotorStart(moto_handle);
    }
}
static void L_Rte_TBotCarSetMotoPwr_RearLeft(Rte_TBotCar *self, Int32 pwr)
{
    Bsp_THandle moto_handle = self->moto_rear_right_; 
    if (pwr == 0)
    {
        Bsp_MotorStop(moto_handle);
    }
    else if (pwr > 0)
    {
        Bsp_MotorSetDirection(moto_handle, False);
        Bsp_MotorSetSpeed(moto_handle, pwr);
        Bsp_MotorStart(moto_handle);
    }
    else // if (pwr < 0)
    {
        Bsp_MotorSetDirection(moto_handle, True);
        Bsp_MotorSetSpeed(moto_handle, -pwr);
        Bsp_MotorStart(moto_handle);
    }
}
static void L_Rte_TBotCarSetMotoPwr_FrontRight(Rte_TBotCar *self, Int32 pwr)
{
    Bsp_THandle moto_handle = self->moto_front_left_;
    if (pwr == 0)
    {
        Bsp_MotorStop(moto_handle);
    }
    else if (pwr > 0)
    {
        Bsp_MotorSetDirection(moto_handle, True);
        Bsp_MotorSetSpeed(moto_handle, pwr);
        Bsp_MotorStart(moto_handle);
    }
    else // if (pwr < 0)
    {
        Bsp_MotorSetDirection(moto_handle, False);
        Bsp_MotorSetSpeed(moto_handle, -pwr);
        Bsp_MotorStart(moto_handle);
    }
}
static void L_Rte_TBotCarSetMotoPwr_RearRight(Rte_TBotCar *self, Int32 pwr)
{
    Bsp_THandle moto_handle = self->moto_front_right_;   /// 
    if (pwr == 0)
    {
        Bsp_MotorStop(moto_handle);
    }
    else if (pwr > 0)
    {
        Bsp_MotorSetDirection(moto_handle, True);
        Bsp_MotorSetSpeed(moto_handle, pwr);
        Bsp_MotorStart(moto_handle);
    }
    else // if (pwr < 0)
    {
        Bsp_MotorSetDirection(moto_handle, False);
        Bsp_MotorSetSpeed(moto_handle, -pwr);
        Bsp_MotorStart(moto_handle);
    }
}
#else
static void L_Rte_TBotCarSetMotoPwr(Rte_TBotCar *self, Bsp_THandle moto_handle, Int32 pwr)
{
// 统一向车头方向转动为正方向
    if (pwr == 0)
    {
        Bsp_MotorStop(moto_handle);
    }
    else if (pwr > 0)
    {
        Bsp_MotorSetDirection(moto_handle, True);
        Bsp_MotorSetSpeed(moto_handle, pwr);
    }
    else // if (pwr < 0)
    {
        Bsp_MotorSetDirection(moto_handle, False);
        Bsp_MotorSetSpeed(moto_handle, -pwr);
    }
}
#endif
// ------------------航向角计算-----------------------------


static Bool L_Rte_TBotCarGetYawAngle(Rte_TBotCar *self, Int32 *ret_yaw)
{
    static UInt32 last_tick = 0;
    UInt32 curr_tick = Osc_SystemTick();
    Bsp_TAxisAngleData *angle_data = self->axis_data_array_;
   
    *ret_yaw = self->yaw_offset + (Int32)((angle_data[0].yaw) * 1000);
    
    if (curr_tick - last_tick >= 60000)
    {
        last_tick = curr_tick;
        self->yaw_offset = *ret_yaw;
        Bsp_AxisCalibrate(self->axis_);
    }
    
    return True;
}    


// -------------------回调事件----------------------------

static void L_Rte_TBotCarDoStateNotify(Bot_TCar *sender, UInt32 state)
{
    // ... todo ...
}

static Bool L_Rte_TBotCarDoGetCurrPlace(Bot_TCar *sender, Bot_TCarPlace *ret_place)
{

    Rte_TBotCar *self = &g_rte_bot_car_;
    
    Bsp_AxisGetData(self->axis_, 
        Bsp_kAxisDataTypeAngle, 
        self->axis_data_array_,
        sizeof(self->axis_data_array_) / sizeof(self->axis_data_array_[0]));

    Bool result1 = L_Rte_TBotCarGetYawAngle(self, &ret_place->d);

    if (ABS(ret_place->d - self->parent_.next_place_.d) > 2000)
    {
        return result1;
    }

    Bool result2 = False;
   
    if (Bsp_LaserGetData(self->laser_, 
        Bsp_kLaserDataTypeCollectBoard, 
        self->laser_data_array_,
        sizeof(self->laser_data_array_) / sizeof(self->laser_data_array_[0])) >= 1)
    {
        Int32 orig_data[4];
        
        orig_data[0] = (Int32)self->laser_data_array_[0].distance1;
        orig_data[1] = (Int32)self->laser_data_array_[0].distance2;
        orig_data[2] = (Int32)self->laser_data_array_[0].distance3;
        orig_data[3] = (Int32)self->laser_data_array_[0].distance4;
        Rte_TBotCarPlaceUpdata(&self->place_calc_, orig_data);
        result2 = Rte_TBotCarPlaceGetPlace(&self->place_calc_, &ret_place->x, &ret_place->y);
    }
    
    return result1 && result2;
}
static Bool L_Rte_TBotCarDoSetCurrPlace(Bot_TCar *sender, const Bot_TCarPlace *place)
{
    // ... todo ...
    return False;
}
static Bool L_Rte_TBotCarDoSetMotivePwr(Bot_TCar *sender, Int32 angle, Int32 distance)
{
    Rte_TBotCar *self = &g_rte_bot_car_;
    Int32 motive_pwr = L_Rte_TBotCarCalcMotivePwr(self, distance);
    
    if (motive_pwr == 0)
    {
        self->motive_x_pwr_ = 0;
        self->motive_y_pwr_ = 0;
    }
    else
    {
        double angleRadians = (double)angle / 1000 * 3.141592654 / 180.0;    
        self->motive_x_pwr_ = (Int32)(motive_pwr * cos(angleRadians));
        self->motive_y_pwr_ = (Int32)(motive_pwr * sin(angleRadians));
    }
    
    return True;
}
static Bool L_Rte_TBotCarDoSetSpinPwr(Bot_TCar *sender, Int32 diff_angle)
{
    Rte_TBotCar *self = &g_rte_bot_car_;
    Bool reset_pid = True;
    
    if (diff_angle == 0)
        self->spin_pwr_ = 0;
    else if (diff_angle > 3500)
        self->spin_pwr_ = 30;
    else if (diff_angle < -3500)
        self->spin_pwr_ = -30;
    else
    {
        self->spin_pwr_ = L_Rte_TBotCarCalcSpinPwr(self, diff_angle) / 1000;
        reset_pid = False;
    }
    
    if (reset_pid)
        Mdw_TPidReset(&self->spin_pid_);
        
    
    return True;
}
static void L_Rte_TBotCarDoRunOnce(Bot_TCar *sender)
{
    Rte_TBotCar *self = &g_rte_bot_car_;

    self->moto_front_left_pwr_  = self->motive_x_pwr_ - self->motive_y_pwr_ - self->spin_pwr_;
    self->moto_front_right_pwr_ = self->motive_x_pwr_ + self->motive_y_pwr_ + self->spin_pwr_;
    self->moto_rear_left_pwr_   = self->motive_x_pwr_ + self->motive_y_pwr_ - self->spin_pwr_;
    self->moto_rear_right_pwr_  = self->motive_x_pwr_ - self->motive_y_pwr_ + self->spin_pwr_;
//    
    #if (RTE_BOT_CAR_MOTO_DIY_CTRL)
    L_Rte_TBotCarSetMotoPwr_FrontLeft(self, self->moto_front_left_pwr_);
    L_Rte_TBotCarSetMotoPwr_FrontRight(self, self->moto_front_right_pwr_);
    L_Rte_TBotCarSetMotoPwr_RearRight(self, self->moto_rear_right_pwr_);
    L_Rte_TBotCarSetMotoPwr_RearLeft(self, self->moto_rear_left_pwr_);
    #else
    L_Rte_TBotCarSetMotoPwr(self, self->moto_front_left_, self->moto_front_left_pwr_);
    L_Rte_TBotCarSetMotoPwr(self, self->moto_front_right_, self->moto_front_right_pwr_);
    L_Rte_TBotCarSetMotoPwr(self, self->moto_rear_right_, self->moto_rear_right_pwr_);
    L_Rte_TBotCarSetMotoPwr(self, self->moto_rear_left_, self->moto_rear_left_pwr_);
    #endif
}
static Bool L_Rte_TBotCarDoCheckPlaceValid(Bot_TCar *sender, const Bot_TCarPlace *in_place)
{
    Bool result = True;

    if (in_place->x <= 0 || in_place->y <= 0)
        result = False;
    else if (in_place->x >= PLAT_X_MAX || in_place->y >= PLAT_Y_MAX)
        result = False;
    else if (in_place->x <= CAB_X_OFFSEET_SHORT && (in_place->y <= (PLAT_Y_MAX - CAB_Y_OFFSEET_LONG) || in_place->y >= CAB_Y_OFFSEET_LONG))
        result = False;
    else if (in_place->x >= (PLAT_X_MAX - CAB_X_OFFSEET_SHORT) && (in_place->y <= (PLAT_Y_MAX - CAB_Y_OFFSEET_LONG) || in_place->y >= CAB_Y_OFFSEET_LONG))
        result = False;
    else if (((in_place->x >= PLAT_X_MAX - CAB_X_OFFSEET_LONG) && (in_place->x <= PLAT_X_MAX - CAB_X_OFFSEET_SHORT)) && (in_place->y <= (PLAT_Y_MAX - CAB_Y_OFFSEET_LONG) || in_place->y >= CAB_Y_OFFSEET_LONG))
        result = False;
        

    return result;
}



// -----------------------------------------------

void Rte_BotCarCreate(void)
{
    Rte_TBotCar *self = &g_rte_bot_car_;
    Int32 result = Bot_TCarCreate(&self->parent_);
    
    if (result >= 0)
    {
        self->moto_front_left_ = Bsp_MotorCreate(Bsp_kMotorIdFrontLeft);
        self->moto_front_right_ = Bsp_MotorCreate(Bsp_kMotorIdFrontRight);
        self->moto_rear_left_ = Bsp_MotorCreate(Bsp_kMotorIdRearLeft);
        self->moto_rear_right_ = Bsp_MotorCreate(Bsp_kMotorIdRearRight);
        
        Mdw_TPidCreate(&self->spin_pid_, 8, 1, 30, 30000, -30000);
        
        self->axis_ = Bsp_AxisCreate(Bsp_kAxisIdWt61c);
        
        self->laser_ = Bsp_LaserCreate(Bsp_kLaserIdCollectBoard);
        
        Rte_TBotCarPlaceCreate(&self->place_calc_);
    }
}

void Rte_BotCarInit(void)
{
    Rte_TBotCar *self = &g_rte_bot_car_;
    Bot_TCar *car_handle = &g_rte_bot_car_.parent_;
    
    car_handle->DoStateNotify = L_Rte_TBotCarDoStateNotify;
    car_handle->DoCheckPlaceValid = L_Rte_TBotCarDoCheckPlaceValid;
    car_handle->DoGetCurrPlace = L_Rte_TBotCarDoGetCurrPlace;
    car_handle->DoSetCurrPlace = L_Rte_TBotCarDoSetCurrPlace;
    car_handle->DoSetSpinPwr = L_Rte_TBotCarDoSetSpinPwr;
    car_handle->DoSetMotivePwr = L_Rte_TBotCarDoSetMotivePwr;
    car_handle->DoRunOnce = L_Rte_TBotCarDoRunOnce;
    
    Bot_TCarSetPlaceMod(car_handle, False /*is_place_mode*/);
    
    Bsp_MotorOpen(g_rte_bot_car_.moto_front_left_);
    Bsp_MotorOpen(g_rte_bot_car_.moto_front_right_);
    Bsp_MotorOpen(g_rte_bot_car_.moto_rear_left_);
    Bsp_MotorOpen(g_rte_bot_car_.moto_rear_right_);
    
    self->spin_pid_.max_err = 1000;
    self->spin_pid_.min_err = -1000;
    self->spin_pid_.max_err_diff = 500;
    self->spin_pid_.min_err_diff = -500;
    self->spin_pid_.max_err_sum = 100;
    self->spin_pid_.min_err_sum = -100;
    
    self->yaw_offset = 0;
    Bsp_AxisSetDataDepth(self->axis_,
        Bsp_kAxisDataTypeAngle, 
        self->axis_data_buf_, 
        sizeof(self->axis_data_buf_) / sizeof(self->axis_data_buf_[0]));
    Bsp_AxisOpen(self->axis_);
    Bsp_AxisCalibrate(self->axis_);
    
    Bsp_LaserSetDataDepth(self->laser_, 
        Bsp_kLaserDataTypeCollectBoard, 
        self->axis_data_buf_, 
        sizeof(self->axis_data_buf_) / sizeof(self->axis_data_buf_[0]));
    Bsp_LaserOpen(self->laser_);
    
    Rte_TBotCarPlaceInit(&self->place_calc_);
    self->place_calc_.owner = self;


    Bot_TCarSetPlaceMod(car_handle, True /*is_place_mode*/);    // 开启位置模式
}

Int32 Rte_BotCarRunOnce(void)
{
    Bsp_AxisRunOnce(g_rte_bot_car_.axis_);
    Bsp_LaserRunOnce(g_rte_bot_car_.laser_);
    Bot_TCarRunOnce(&g_rte_bot_car_.parent_);

    return 10;
}

void Rte_BotCarDone(void)
{
    Rte_TBotCarPlaceDone(&g_rte_bot_car_.place_calc_);
    
    Bsp_LaserClose(g_rte_bot_car_.laser_);
    
    Bsp_AxisClose(g_rte_bot_car_.axis_);
    
    Bsp_MotorStop(g_rte_bot_car_.moto_front_left_);
    Bsp_MotorStop(g_rte_bot_car_.moto_front_right_);
    Bsp_MotorStop(g_rte_bot_car_.moto_rear_left_);
    Bsp_MotorStop(g_rte_bot_car_.moto_rear_right_);
    
    Bsp_MotorClose(g_rte_bot_car_.moto_front_left_);
    Bsp_MotorClose(g_rte_bot_car_.moto_front_right_);
    Bsp_MotorClose(g_rte_bot_car_.moto_rear_left_);
    Bsp_MotorClose(g_rte_bot_car_.moto_rear_right_);
}

void Rte_BotCarDestroy(void)
{
    Rte_TBotCarPlaceDestroy(&g_rte_bot_car_.place_calc_);
    
    Bsp_LaserFree(g_rte_bot_car_.laser_);
    
    Bsp_AxisFree(g_rte_bot_car_.axis_);
    
    Mdw_TPidDestroy(&g_rte_bot_car_.spin_pid_);
    
    Bsp_MotorFree(g_rte_bot_car_.moto_front_left_);
    Bsp_MotorFree(g_rte_bot_car_.moto_front_right_);
    Bsp_MotorFree(g_rte_bot_car_.moto_rear_left_);
    Bsp_MotorFree(g_rte_bot_car_.moto_rear_right_);
    
    Bot_TCarDestroy(&g_rte_bot_car_.parent_);
}


Bot_TCar *Rte_BotCarHandle(void)
{
    Bot_TCar *result = NULL;
    
    if ((g_rte_bot_car_.parent_.state & Bot_kCarStateMaskValid) != 0)
    {
        result = &g_rte_bot_car_.parent_;
    }
    
    return result;
}


///< Generated on "2024-04-13 11:03:07" by the tool "gen_hq_file.py >> V20231119" 

